from MotorStub import MotorStub
from Drawing import Drawing
from DebugTrace import DebugTrace

class MotorController:
    def __init__(self,debugLevel=1):
        self.tool=0
        self.motors= {"X":None, "Y":None, "Z":None, "E0":None, "E1":None}
        self.drawGraphics=None
        self.trace = DebugTrace("MotorController",debugLevel)

    def enableDrawing(self,scaling=1):
        self.trace.trace(2,"Drawing enabled, scaling="+str(scaling))
        self.drawGraphics = Drawing(self.trace.debugLevel,scaling)

    def disableDrawing(self):
        self.trace.trace(2,"Drawing disabled")
        self.drawGraphics=None

    def getCurrentToolMotor(self):
        if (self.tool==0):
            self.trace.trace(3,"Tool 0")
            return self.motors["E0"]
        elif (self.tool==1):
            self.trace.trace(3,"Tool 1")
            return self.motors["E1"]
        else:
            self.trace.trace(1,"Invalid tool number "+str(self.tool))
            return None #Failure

    def setTool(self,tool):
        if (tool==0 or tool==1):
            self.tool=tool
            self.trace.trace(2,"Setting tool to "+str(tool))
            return "ok"
        else:
            self.trace.trace(1,"Invalid tool number "+str(tool))
            return "!!"

    def move(self,x,y,z,e):
        self.trace.trace(2,"Moving to X:"+str(x)+" Y:"+str(y)+" Z:"+str(z)+" E:"+str(e))

        self.motors["X"].move(x)
        self.motors["Y"].move(y)
        self.motors["Z"].move(z)

        eMotor=self.getCurrentToolMotor()
        if(eMotor==None):
            self.trace.trace(1,"No motor found for current tool!")
            return "!!"
                    
        eMotor.move(e)

        if (self.drawGraphics!=None):
            newX = self.motors["X"].getPosition()
            newY = self.motors["Y"].getPosition()
            newZ = self.motors["Z"].getPosition() 
            if (e!=None):
                self.drawGraphics.drawTo(newX,newY,newZ)
            else:
                self.drawGraphics.moveTo(newX,newY,newZ)

        return "ok"
